#!/usr/bin/env python
import asyncio
import logging
from bleak import BleakScanner, BleakClient
from bleak.exc import BleakError
import rospy
import json
import time
from std_msgs.msg import String
from pi_driver.srv import SetString, SetStringResponse, GetString, GetStringResponse

class BLEROSNode:
    def __init__(self):
        self.loop = asyncio.new_event_loop()
        asyncio.set_event_loop(self.loop)
        self.client = None
        self.connected = False
        self.notification_pub = rospy.Publisher('~ble_notifications', String, queue_size=10)
        
        # 初始化服务
        self.scan_srv = rospy.Service('~ble_scan', GetString, self.handle_scan)
        self.connect_srv = rospy.Service('~ble_connect', SetString, self.handle_connect)
        self.connect_srv = rospy.Service('~ble_is_connected', GetString, self.handle_is_connected)
        self.disconnect_srv = rospy.Service('~ble_disconnect', SetString, self.handle_disconnect)
        self.discover_srv = rospy.Service('~ble_discover_services', GetString, self.handle_discover_services)
        self.read_srv = rospy.Service('~ble_read_characteristic', GetString, self.handle_read_characteristic)
        self.write_srv = rospy.Service('~ble_write_characteristic', SetString, self.handle_write_characteristic)
        self.subscribe_srv = rospy.Service('~ble_subscribe_notification', GetString, self.handle_subscribe_notification)
        self.unsubscribe_srv = rospy.Service('~ble_unsubscribe_notification', GetString, self.handle_unsubscribe_notification)
        
        rospy.loginfo("BLE ROS 节点已启动")

    def notification_handler(self, sender, data):
        """处理BLE通知的回调函数"""
        msg = f"{sender}: {data.hex()}"
        self.notification_pub.publish(json.dumps(list(data)))
        rospy.loginfo(f"收到BLE通知: {msg}")

    async def _scan_devices(self, timeout=5.0):
        """异步扫描设备"""
        try:
            devices = await BleakScanner.discover(timeout=timeout)
            return devices
        except Exception as e:
            rospy.logerr(f"扫描失败: {e}")
            return []

    def handle_scan(self, req):
        """处理扫描请求"""
        rospy.loginfo("开始扫描BLE设备...")
        
        # 在同步代码中运行异步函数
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        devices = self.loop.run_until_complete(self._scan_devices())
        
        response = GetStringResponse()
        if not devices:
            rospy.loginfo("没有发现任何BLE设备")
            return response
        
        items = []
        for device in devices:
            items.append({
                "address": device.address,
                "name": device.name if device.name else device.address,
                "rssi": device.rssi,
            })
            # response.addresses.append(device.address)
            # response.names.append(device.name if device.name else "Unknown")
            # response.rssis.append(device.rssi)
        response.data = json.dumps(items)
        rospy.loginfo(f"发现 {len(items)} 个设备")
        return response

    def is_connected(self):
        return self.client is not None and self.client.is_connected

    def handle_is_connected(self, req):
        if self.is_connected():
            return GetStringResponse('true')
        else:
            return GetStringResponse('false')

    async def _connect_device(self, address):
        """异步连接设备"""
        try:
            self.client = BleakClient(address)
            await self.client.connect()
            return True, "连接成功"
        except BleakError as e:
            return False, f"连接失败: {e}"

    def handle_connect(self, req):
        """处理连接请求"""
        if self.is_connected():
            return SetStringResponse("已连接到设备，请先断开当前连接")
        
        rospy.loginfo(f"尝试连接设备: {req.data}")
        
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        success, message = self.loop.run_until_complete(self._connect_device(req.data))
        
        rospy.loginfo(message)
        return SetStringResponse(message)

    async def _disconnect_device(self):
        """异步断开连接"""
        try:
            if self.is_connected():
                await self.client.disconnect()
                self.client = None
                return True, "已断开连接"
            return False, "没有活动的连接"
        except BleakError as e:
            return False, f"断开连接失败: {e}"

    def handle_disconnect(self, req):
        """处理断开连接请求"""
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        # self.loop.stop()
        success, message = self.loop.run_until_complete(self._disconnect_device())
        
        rospy.loginfo(message)
        return SetStringResponse(message)

    async def _discover_services(self):
        """异步发现服务"""
        if not self.is_connected():
            return [], [], [], "未连接任何设备"
        
        try:
            services = await self.client.get_services()
            service_uuids = []
            char_uuids = []
            properties = []
            
            for service in services:
                service_uuids.append(str(service.uuid))
                for char in service.characteristics:
                    char_uuids.append(str(char.uuid))
                    properties.append(",".join(char.properties))
            
            return service_uuids, char_uuids, properties, "成功"
        except BleakError as e:
            return [], [], [], f"发现服务失败: {e}"

    def handle_discover_services(self, req):
        """处理发现服务请求"""
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        service_uuids, char_uuids, properties, message = self.loop.run_until_complete(self._discover_services())
        
        response = {}
        response['service_uuids'] = service_uuids
        response['characteristic_uuids'] = char_uuids
        response['properties'] = properties
        
        rospy.loginfo(message)
        return GetStringResponse(json.dumps(response))

    async def _read_characteristic(self, char_uuid):
        """异步读取特征"""
        if not self.is_connected():
            return False, None, "未连接任何设备"
        
        try:
            value = await self.client.read_gatt_char(char_uuid)
            return True, value, "读取成功"
        except BleakError as e:
            return False, None, f"读取失败: {e}"

    def handle_read_characteristic(self, req):
        """处理读取特征请求"""
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        success, value, message = self.loop.run_until_complete(self._read_characteristic(req.data))
        
        res = {}
        res["success"] = success
        res["value"] = list(value) if value else []
        res["message"] = message
        
        rospy.loginfo(f"{message} (特征: {req.data})")
        return GetStringResponse(json.dumps(res))

    async def _write_characteristic(self, char_uuid, data, response_needed):
        """异步写入特征"""
        if not self.is_connected():
            return False, "未连接任何设备"
        
        try:
            await self.client.write_gatt_char(char_uuid, data, response=response_needed)
            return True, "写入成功"
        except BleakError as e:
            return False, f"写入失败: {e}"

    def handle_write_characteristic(self, req):
        """处理写入特征请求"""
        params = json.loads(req.data)
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        success, message = self.loop.run_until_complete(
            self._write_characteristic(params["uuid"], bytes(params["value"]), True)
        )
        
        rospy.loginfo(f"{message} (特征: {params['uuid']})")
        return SetStringResponse(message)

    async def _subscribe_notification(self, char_uuid):
        """异步订阅通知"""
        if not self.is_connected():
            return False, "未连接任何设备"
        
        try:
            await self.client.start_notify(char_uuid, self.notification_handler)
            return True, "订阅成功"
        except BleakError as e:
            return False, f"订阅失败: {e}"

    def handle_subscribe_notification(self, req):
        """处理订阅通知请求"""
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        try:
            params = json.loads(req.data)
            if "uuid" in params:
                success, message = self.loop.run_until_complete(self._subscribe_notification(params["uuid"]))
                self.loop.run_forever()
                rospy.loginfo(f"{message} (特征: {req.data})")
                return GetStringResponse(message)
        except Exception as e:
            print(e)
            return GetStringResponse()
        return GetStringResponse()

    async def _unsubscribe_notification(self, char_uuid):
        """取消异步订阅通知"""
        if not self.is_connected():
            return False, "未连接任何设备"
        
        try:
            await self.client.stop_notify(char_uuid)
            return True, "取消订阅成功"
        except BleakError as e:
            return False, f"取消订阅失败: {e}"

    def handle_unsubscribe_notification(self, req):
        """处理取消订阅通知请求"""
        # loop = asyncio.new_event_loop()
        # asyncio.set_event_loop(loop)
        try:
            params = json.loads(req.data)
            self.loop.stop()
            time.sleep(1)
            if "uuid" in params:
                success, message = self.loop.run_until_complete(self._unsubscribe_notification(params["uuid"]))
                rospy.loginfo(f"{message} (特征: {req.data})")
                return GetStringResponse(message)
        except Exception as e:
            print(e)
            return GetStringResponse()
        return GetStringResponse()

if __name__ == '__main__':
    rospy.init_node('ble_ros_node')
    node = BLEROSNode()
    rospy.spin()